#include "motor.h"
#include "basic.h"

#define MOTOR_PWM_PERIOD_COUNT (500)

#define MOTOR_MAX_TORQUE (100)
// #define MOTOR_DETEC_MAX_SPEED (2500)
#define MOTOR_DETEC_MAX_SPEED (800)

#define USE_FRONT_WHEEL 0

void motor_init(void) {
#if USE_FRONT_WHEEL
  NVIC_EnableIRQ(ENC_GPIOA_INT_IRQN);
  NVIC_EnableIRQ(ENC_GPIOB_INT_IRQN);
#else
  NVIC_EnableIRQ(ENC_INT_IRQN);
#endif

  DL_Timer_startCounter(MOTOR_PWM_R_INST);
}
void motor_set_torque(motor_position_t pos, int16_t torque) {
  int16_t duty = MOTOR_PWM_PERIOD_COUNT -
                 torque * MOTOR_PWM_PERIOD_COUNT / MOTOR_MAX_TORQUE;
  GPTIMER_Regs *set_pwm_timer = MOTOR_PWM_R_INST;
  DL_TIMER_CC_INDEX set_channel =
      (pos & MOTOR_LEFT) ? DL_TIMER_CC_0_INDEX : DL_TIMER_CC_1_INDEX;

  if (duty > MOTOR_PWM_PERIOD_COUNT)
    duty = MOTOR_PWM_PERIOD_COUNT;
  else if (duty < 0)
    duty = 0;

  DL_TimerG_setCaptureCompareValue(set_pwm_timer, duty, set_channel);
}

void motor_set_mode(motor_position_t pos, motor_mode_t mode) {

  uint32_t in1 = 0;
  uint32_t in2 = 0;

  if (pos & MOTOR_FRONT) {
    // if (pos & MOTOR_LEFT)
    // {
    //   in1 = MOTOR_FL_IN1_PIN;
    //   in2 = MOTOR_FL_IN2_PIN;
    // }
    // else
    // {
    //   // in1 = MOTOR_FR_IN1_PIN;
    //   // in2 = MOTOR_FR_IN2_PIN;
    // }
  } else {
    if (pos & MOTOR_LEFT) {
      in1 = MOTOR_RL_IN1_PIN;
      in2 = MOTOR_RL_IN2_PIN;
    } else {
      in1 = MOTOR_RR_IN1_PIN;
      in2 = MOTOR_RR_IN2_PIN;
    }
  }

  /*
              in1     in2
  forward     1       0
  backward    0       1
  stop        1       1
  neutral     0       0
  */
  switch (mode) {
  default:
  case MOTOR_NEUTRAL:
    DL_GPIO_clearPins(MOTOR_PORT, in1);
    DL_GPIO_clearPins(MOTOR_PORT, in2);
    break;
  case MOTOR_FORWARD:
    DL_GPIO_setPins(MOTOR_PORT, in1);
    DL_GPIO_clearPins(MOTOR_PORT, in2);
    break;
  case MOTOR_BACKWARD:
    DL_GPIO_clearPins(MOTOR_PORT, in1);
    DL_GPIO_setPins(MOTOR_PORT, in2);
    break;
  case MOTOR_STOP:
    DL_GPIO_setPins(MOTOR_PORT, in1);
    DL_GPIO_setPins(MOTOR_PORT, in2);
    break;
  }
}
///////////////////SPEED GET SECTION////////////////////////
///////////////////SPEED GET SECTION////////////////////////
///////////////////SPEED GET SECTION////////////////////////

#define USE_T_SPEED_MEASURE
// #define USE_M_SPEED_MEASURE

#ifdef USE_T_SPEED_MEASURE
#define REDUCTION_RATIO 21.3f
#define LINE_COUNT 22.0f

volatile uint64_t rl_last_us = 0, rr_last_us = 0;
volatile uint64_t rl_cur_us = 0, rr_cur_us = 0;
volatile int8_t rl_direction = 1, rr_direction = 1;

float motor_get_speed(motor_position_t pos) {

#if USE_FRONT_WHEEL
  NVIC_DisableIRQ(ENC_GPIOA_INT_IRQN);
  NVIC_DisableIRQ(ENC_GPIOB_INT_IRQN);
#else
  NVIC_DisableIRQ(ENC_INT_IRQN);
#endif

  if (pos & MOTOR_FRONT) // front motors are not supported yet
    return 0;

  uint64_t cur_us = 0;
  float T = 0, speed = 0;

  mspm0_get_clock_us(&cur_us);

#define _max(a, b) ((a) > (b) ? (a) : (b))

  if (pos & MOTOR_LEFT) {
    T = _max(rl_cur_us - rl_last_us, cur_us - rl_cur_us) / 1.0e6f;
    speed = 360.0f * rl_direction / T / REDUCTION_RATIO / LINE_COUNT;
  } else if (pos & MOTOR_RIGHT) {
    T = _max(rr_cur_us - rr_last_us, cur_us - rr_cur_us) / 1.0e6f;
    speed = 360.0f * rr_direction / T / REDUCTION_RATIO / LINE_COUNT;
  }

  // 为了和力矩相对应 我们将速度归一化到MOTOR_MAX_TORQUE
  speed *= (float)MOTOR_MAX_TORQUE / MOTOR_DETEC_MAX_SPEED;

#if USE_FRONT_WHEEL
  NVIC_EnableIRQ(ENC_GPIOA_INT_IRQN);
  NVIC_EnableIRQ(ENC_GPIOB_INT_IRQN);
#else
  NVIC_EnableIRQ(ENC_INT_IRQN);
#endif
  return speed;
}
void motor_irq_handler(void) {
  uint32_t enc_rl_interrup =
      DL_GPIO_getRawInterruptStatus(ENC_RL_OUT_A_PORT, ENC_RL_OUT_A_PIN);
  uint32_t enc_rr_interrup =
      DL_GPIO_getRawInterruptStatus(ENC_RR_OUT_A_PORT, ENC_RR_OUT_A_PIN);
  if (enc_rl_interrup & ENC_RL_OUT_A_PIN) { // rising interrupt
    if (DL_GPIO_readPins(ENC_RL_OUT_B_PORT, ENC_RL_OUT_B_PIN) &
        ENC_RL_OUT_B_PIN)
      rl_direction = -1;
    else
      rl_direction = 1;

    rl_last_us = rl_cur_us;
    mspm0_get_clock_us((uint64_t *)&rl_cur_us);
  }

  if (enc_rr_interrup & ENC_RR_OUT_A_PIN) {
    if (DL_GPIO_readPins(ENC_RR_OUT_B_PORT, ENC_RR_OUT_B_PIN) &
        ENC_RR_OUT_B_PIN)
      rr_direction = 1;
    else
      rr_direction = -1;

    rr_last_us = rr_cur_us;
    mspm0_get_clock_us((uint64_t *)&rr_cur_us);

    // my_printf("rr interrupt detected");
    // my_printf("direction: %d , last: %d, cur: %d\n", rr_direction,
    // rr_last_us, rr_cur_us);
  }

  DL_GPIO_clearInterruptStatus(ENC_RL_OUT_A_PORT,
                               enc_rl_interrup & ENC_RL_OUT_A_PIN);
  DL_GPIO_clearInterruptStatus(ENC_RR_OUT_A_PORT,
                               enc_rr_interrup & ENC_RR_OUT_A_PIN);
}

#endif

void GROUP1_IRQHandler(void) { motor_irq_handler(); }
